#include "HallSensor.h"
#include "Simple_time.h"
const int8_t ELECTRIC_SECTORS[8] = { -1,  0,  4,  5,  2,  1,  3 , -1 };
/**
 * Updates the state and sector following an interrupt
 */
void HallSensor_updateState(Simple_HallSensor_t *handle,uint8_t state)
{
  long new_pulse_timestamp = Simple_micros();

	//int8_t new_hall_state=handle->getHallState();
	int8_t new_hall_state=state;

  // glitch avoidance #1 - sometimes we get an interrupt but pins haven't changed
  if (new_hall_state ==handle-> hall_state) {
    return;
  }
  handle->hall_state = new_hall_state;

  int8_t new_electric_sector = ELECTRIC_SECTORS[handle->hall_state];
  if (new_electric_sector - handle->electric_sector > 3) {
    //underflow
    handle->direction = Direction_CCW;
    handle->electric_rotations += handle->direction;
  }
	else if (new_electric_sector - handle->electric_sector < (-3)) 
	{
    //overflow
    handle->direction = Direction_CW;
    handle->electric_rotations += handle->direction;
  } 
	else 
	{
    handle->direction = (new_electric_sector > handle->electric_sector)? Direction_CW : Direction_CCW;
  }
  handle->electric_sector = new_electric_sector;

  // glitch avoidance #2 changes in direction can cause velocity spikes.  Possible improvements needed in this area
  if (handle->direction == handle->old_direction) {
    // not oscilating or just changed direction
    handle->pulse_diff = new_pulse_timestamp - handle->pulse_timestamp;
  } else {
    handle->pulse_diff = 0;
  }

  handle->pulse_timestamp = new_pulse_timestamp;
  handle->old_direction = handle->direction;
  //if (onSectorChange != nullptr) onSectorChange(electric_sector);
}



/*
	Shaft angle calculation
  TODO: numerical precision issue here if the electrical rotation overflows the angle will be lost
*/
float HallSensor_getMechanicalAngle(Simple_HallSensor_t *hanle)
{
  return ((float)((hanle->electric_rotations * 6 + hanle->electric_sector) % hanle->cpr) / (float)hanle->cpr) * _2PI ;
}

/*
  Shaft velocity calculation
  function using mixed time and frequency measurement technique
*/
float HallSensor_getVelocity(Simple_HallSensor_t *handle)
{
  long last_pulse_diff = handle->pulse_diff;
  if (last_pulse_diff == 0)// || ((long)(Simple_micros() - handle->pulse_timestamp) > last_pulse_diff) ) { // last velocity isn't accurate if too old
	{
		return 0;
  } 
	else 
	{
    float vel = handle->direction * (_2PI / (float)handle->cpr) / (last_pulse_diff / 1000000.0f);
    // quick fix https://github.com/simplefoc/Arduino-FOC/issues/192
    if(vel < -handle->velocity_max || vel > handle->velocity_max)  vel = 0.0f;   //if velocity is out of range then make it zero
    return vel;
  }

}



float HallSensor_getAngle(Simple_HallSensor_t *handle) 
{
  return ((float)(handle->electric_rotations * 6 + handle->electric_sector) / (float)handle->cpr) * _2PI ;
}


float HallSensor_getSensorAngle(Simple_HallSensor_t *handle)
{
  return HallSensor_getAngle(handle);
}


double HallSensor_getPreciseAngle(Simple_HallSensor_t *handle) 
{
  return ((double)( handle->electric_rotations * 6 + handle->electric_sector) / (double)handle->cpr) * (double)_2PI ;
}


int32_t HallSensor_getFullRotations(Simple_HallSensor_t *handle) 
{
  return (int32_t)(( handle->electric_rotations * 6 + handle->electric_sector) / handle->cpr);
}


// HallSensor initialisation of the hardware pins 
// and calculation variables
void HallSensor_init(Simple_HallSensor_t *handle,uint8_t pp)
{
	handle->cpr=pp*6;
  // initialise the electrical rotations to 0
  handle->electric_rotations = 0;
  //HallSensor_updateState(handle);
	handle->getHallState();
  handle->pulse_timestamp = Simple_micros();

  // we don't call Sensor::init() here because init is handled in HallSensor class.
}
